package com.googlecode.mbhsrobopro.controller;

import java.awt.Point;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.Timer;
import java.util.TimerTask;

import javax.swing.JOptionPane;

import net.java.games.input.Controller;
import net.java.games.input.ControllerEnvironment;

import com.googlecode.mbhsrobopro.controller.components.DynamicGraph;
import com.googlecode.mbhsrobopro.controller.components.ProgramController;
import com.googlecode.mbhsrobopro.controller.gamepad.PS2Controller;

import edu.mbhs.fischertechnik.FTController;
import edu.mbhs.fischertechnik.Input;
import edu.mbhs.fischertechnik.Motor;

/**
 * Establishes the rover connection and changes motor settings based on keyboard events. Also periodically
 * collects thermistor readings.
 * 
 * @author Brian Clanton
 *
 */

public class Test {

	public static final String[] labels = {"Time", "Thermistor", "Left Photocell", 
		"Right Photocell", "Center Photocell"};
	public static final int NUM_LABELS = labels.length;
	public static HashMap<String, ArrayList<Double>> data = new HashMap<String, ArrayList<Double>>();

	public static final int DELAY = 1000;
	public static final int INTERVAL = 1000;
	public static final double THRESHOLD_ANGLE = Math.toRadians(15);

	private static boolean ps2Control = false;

	public static void main(String[] args) {
		PS2Controller.initDirectionMap();

		PS2Controller ps2 = null;
		ProgramController p = new ProgramController();

		final FTController c = new FTController();
		c.openController("COM17");

		// Controller Initialization
		int option = JOptionPane.showConfirmDialog(null, "Look for a PS2 Controller?", "Control Scheme", JOptionPane.YES_NO_OPTION);

		if (option == JOptionPane.YES_OPTION) {
			ControllerEnvironment ce = ControllerEnvironment.getDefaultEnvironment();

			for (Controller co : ce.getControllers())
				if (co.getType() == Controller.Type.GAMEPAD || co.getType() == Controller.Type.STICK) {
					ps2 = new PS2Controller(co);
					System.out.println("Controller Found");
					ps2Control = true;
					break;
				}

			if (ps2 == null)
				System.out.println("No Controller Found");
		}

		// Periodic Data Collect
		Timer timer = new Timer();

		final double iT = System.currentTimeMillis() / 1000.0;

		timer.scheduleAtFixedRate(new TimerTask() {
			public void run() {
				addDataPoint(labels[1], (double) c.getAnalog(Input.I5));
				addDataPoint(labels[2], (double) c.getAnalog(Input.I2));
				addDataPoint(labels[3], (double) c.getAnalog(Input.I3));
				addDataPoint(labels[4], (double) c.getAnalog(Input.I1));
				DynamicGraph.values.add(new Point.Double(System.currentTimeMillis() / 1000.0 - iT, getLastDataPoint(labels[4])));
			}
		}, DELAY, INTERVAL);

		// Rover Control
		while (true) {
			p.setThermistorReading(c.getAnalog(Input.I1));
			p.setPhotocellReading(c.getAnalog(Input.I2), ProgramController.FRONT_LEFT);
			p.setPhotocellReading(c.getAnalog(Input.I3), ProgramController.FRONT_RIGHT);

			c.setMotor(p.getLightValue(), Motor.M4);

			if (ps2Control) {
				ps2.pollController();
				
				if (ps2.getDPadDirection() != PS2Controller.Direction.NEUTRAL) {
					switch (ps2.getDPadDirection()) {
					case NORTH:
						c.setMotor(p.getMotorSpeed(), Motor.M1);
						c.setMotor(p.getMotorSpeed(), Motor.M2);
						break;
						
					case SOUTH:
						c.setMotor(-p.getMotorSpeed(), Motor.M1);
						c.setMotor(-p.getMotorSpeed(), Motor.M2);
						c.setMotor(512, Motor.M3);
						break;
						
					case EAST:
						c.setMotor(-p.getMotorSpeed(), Motor.M1);
						c.setMotor(p.getMotorSpeed(), Motor.M2);
						break;
						
					case WEST:
						c.setMotor(p.getMotorSpeed(), Motor.M1);
						c.setMotor(-p.getMotorSpeed(), Motor.M2);
						break;
					}
				} else {
					
				}
			} else {
				if (p.isNormalControl()) {
					if (p.isForward()) {
						c.setMotor(p.getMotorSpeed(), Motor.M1);
						c.setMotor(p.getMotorSpeed(), Motor.M2);
						if (p.isRight()) {
							c.setMotor((int)(p.getMotorSpeed() * p.getScale()), Motor.M1);
						}
						else if (p.isLeft()) {
							c.setMotor((int)(p.getMotorSpeed() * p.getScale()), Motor.M2);
						}
					} else if (p.isBackward()) {
						c.setMotor(-p.getMotorSpeed(), Motor.M1);
						c.setMotor(-p.getMotorSpeed(), Motor.M2);
						c.setMotor(512, Motor.M3);
						if (p.isRight()) {
							c.setMotor((int)(p.getMotorSpeed() * p.getScale()), Motor.M1);
						}
						else if (p.isLeft()) {
							c.setMotor((int)(p.getMotorSpeed() * p.getScale()), Motor.M2);
						}

					} else if (p.isRight()) {
						c.setMotor(-p.getMotorSpeed(), Motor.M1);
						c.setMotor(p.getMotorSpeed(), Motor.M2);

					} else if (p.isLeft()) {
						c.setMotor(p.getMotorSpeed(), Motor.M1);
						c.setMotor(-p.getMotorSpeed(), Motor.M2);
					} else {
						c.setMotor(0, Motor.M1);
						c.setMotor(0, Motor.M2);
						c.setMotor(0, Motor.M3);
					}
				} else if (p.isLineControl()) {
					int left = c.getAnalog(Input.I2);
					int right = c.getAnalog(Input.I3);

					if (left <= 1250) {
						c.setMotor((int) (-p.getMotorSpeed() * .6), Motor.M1);
						c.setMotor((int) (p.getMotorSpeed() * .6), Motor.M2);	
					} else if (right <= 1100) {
						c.setMotor((int) (p.getMotorSpeed() * .6), Motor.M1);
						c.setMotor((int) (-p.getMotorSpeed() * .6), Motor.M2);				
					} else {
						c.setMotor((int) (p.getMotorSpeed() * .5), Motor.M1);
						c.setMotor((int) (p.getMotorSpeed() * .5), Motor.M2);
					}
				}
			}
		}
	}

	public static String getHeader() {
		String header = "";

		for (String s : labels)
			header += s + "\t";

		return header.substring(0, header.lastIndexOf("\t"));
	}

	public static void addDataPoint(String label, double value) {
		if(data.size() >= 1)
			data.get(label).add(value);
	}

	public static double getLastDataPoint(String label) {
		return data.get(label).size() == 0 ? 0 : data.get(label).get(data.get(label).size() - 1);
	}
}
